#include <SoftwareSerial.h>
#include <ax12.h>
#define STR 1

//SoftwareSerial mySerial = SoftwareSerial(2, 3);
AX12 motor;

void setup() {
  
  //pinMode(2, INPUT);
  //pinMode(3,OUTPUT);
  //mySerial.begin(9600);
  motor = AX12();
 

  Serial.begin (115200);  // inicializa el Serial a 115,2 Kb/s
  AX12::init (1000000);   // inicializa los AX12 a 1 Mb/s
  Serial.flush();

  byte detect;          
  byte num = AX12::autoDetect (&detect, 1); // detección de IDs

  Serial.print (" deteccion: ");
  Serial.println (num, DEC);

  motor.id = detect; // asigna las ID detectadas a los motores definidos previamente
  motor.SRL = RETURN_ALL;
  
  Serial.print (" ID detectado: ");
  Serial.println (detect, DEC);
  Serial.print (" delay: ");
  AX12info test = motor.readInfo (RETURN_DELAY_TIME);
  Serial.println (test.value, DEC);
  Serial.print (" error lectura: ");
  Serial.println (test.error, DEC);

}

void loop() {
    
 int *command = 0;
	// send data only when you receive data:


    char buffer[STR];
    int value [3];
    int last_val = 0; 

        int i=0;
  
	if (Serial.available() >= STR) {

                for (i=0;i<STR;i++) {
                    buffer[i]=Serial.read();
                }
                
                if (buffer[0] == 'A')   //Go to arm movements
                {
                  if (buffer[1] == 's')
                  {
                    for (int j = 2; j<5;j++)
                    {
                      value [j-2] = atoi(&buffer[j]);
                    }
                    last_val = last_val + value [0] * 100;
                    last_val = last_val + value [1] * 10;
                    last_val = last_val + value [2];    
                  }
                
                }
                else
                {
                Serial.print(buffer);
                }     
	}

        

    //motor.setVel (random (100, 300));
    //motor.setPos (random (200, 800));
    
     //delay (100);
    
    //int pos = motor.getPos(); 
    //int vel = motor.getSpeed(); 
    //int load = motor.getLoad();
    
    //Serial.println (" ");
    //Serial.print (" posicion: ");
    //Serial.println (pos, DEC);
    //Serial.print (" velocidad: ");
    //Serial.println (vel, DEC);
    //Serial.print (" carga: ");
    //Serial.println (load, DEC);
        
    //delay (1100);
  
}
